﻿#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QMessageBox>

#pragma execution_character_set("utf-8")
MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    setFocusPolicy(Qt::FocusPolicy::ClickFocus);
    this->setWindowTitle("基于物理引擎的月面移动机器人仿真系统");
    this->initStyle();


    ui->groupBox->setStyleSheet("QGroupBox { font-weight: bold;color: #FFFFFF } ");
    ui->groupBox_2->setStyleSheet("QGroupBox { font-weight: bold;color: #FFFFFF } ");
    ui->groupBox_3->setStyleSheet("QGroupBox { font-weight: bold;color: #FFFFFF } ");
    ui->lineEdit_sx->setStyleSheet("QLineEdit:disabled { color: #386487; }");
    ui->lineEdit_sy->setStyleSheet("QLineEdit:disabled { color: #386487; }");
    ui->lineEdit_ex->setStyleSheet("QLineEdit:disabled { color: #386487; }");
    ui->lineEdit_ey->setStyleSheet("QLineEdit:disabled { color: #386487; }");
    ui->lineEdit_velocity->setStyleSheet("QLineEdit:disabled { color: #386487; }");
    ui->lineEdit_px->setStyleSheet("QLineEdit:disabled { color: #386487; }");
//    ui->lineEdit_py->setStyleSheet("QLineEdit:disabled { color: #386487; }");
    ui->lineEdit_headingAngle->setStyleSheet("QLineEdit:disabled { color: #386487; }");

//    ui->pushButton_planTraj->setStyleSheet("QPushButton::icon{margin-right: 5px;}");

    m_thread = new AlgoThread();
    m_thread->start();






    myWorld=GameWorld::getHandle()->myWorld;
//    osg_widget=ui->openGLWidget;
    osg_widget=ui->openGLWidget;
    ui->openGLWidget->setFocusPolicy(Qt::StrongFocus);
    myVehicle=GameWorld::getHandle()->myVehicle;
    viewer=osg_widget->getViewer();
    GameWorld::getHandle()->setViewer(viewer);


//    这里貌似没有什么用
//    btosgVec3 up(0., 0., 1.);
    osg::Matrix matrix;
//    matrix.makeLookAt(osg::Vec3(0.,8.,5.), osg::Vec3(0.,0.,1.), up);

    viewer->getCamera()->setViewMatrix(matrix);
    viewer->addEventHandler(new osgViewer::StatsHandler);


    osg::ref_ptr<osg::Texture2D> texture = new osg::Texture2D;
    osg::ref_ptr<osg::Image> image = osgDB::readImageFile( "assets/img/xingKong02.jpeg" );
    texture->setImage( image.get() );

    osg::ref_ptr<osg::Drawable> quad = osg::createTexturedQuadGeometry(osg::Vec3(), osg::Vec3(1.0f, 0.0f, 0.0f), osg::Vec3(0.0f, 1.0f, 0.0f) );
    quad->getOrCreateStateSet()->setTextureAttributeAndModes( 0,texture.get());

    osg::ref_ptr<osg::Geode> geode = new osg::Geode;
    geode->addDrawable( quad.get() );

    //这里是我的背景
    osg::ref_ptr<osg::Camera> camera = new osg::Camera;
    camera->setClearMask( 0 );
    camera->setCullingActive( false );
    camera->setAllowEventFocus( false );
    camera->setReferenceFrame( osg::Transform::ABSOLUTE_RF );
    camera->setRenderOrder( osg::Camera::POST_RENDER );
    camera->setProjectionMatrix( osg::Matrix::ortho2D(0.0, 1.0, 0.0, 1.0) );
    camera->addChild( geode.get() );
    // 创建一个新的着色器程序
    osg::ref_ptr<osg::Program> program = new osg::Program;
    program->addShader(osgDB::readShaderFile(osg::Shader::FRAGMENT, ":/brighten/brighten.frag"));

    // 创建一个状态集并设置着色器程序
    osg::ref_ptr<osg::StateSet> stateSet = geode->getOrCreateStateSet();
    stateSet->setAttributeAndModes(program.get());

    // 设置纹理
    stateSet->setTextureAttributeAndModes(0, texture.get());

    // 设置亮度调整参数的统一变量
    osg::ref_ptr<osg::Uniform> brightnessUniform = new osg::Uniform("brightness", 0.9f); // 增加的亮度值
    stateSet->addUniform(brightnessUniform.get());


    osg::StateSet* ss = camera->getOrCreateStateSet();
    ss->setMode( GL_LIGHTING, osg::StateAttribute::OFF );
    ss->setAttributeAndModes( new osg::Depth(osg::Depth::LEQUAL, 1.0, 1.0) );
    myWorld->scene->addChild(camera.get());






    viewer->getCamera()->addChild( myWorld->scene );

    viewer->setCameraManipulator( GameWorld::getHandle()->manipulator1 );
    viewer->addEventHandler(new FollowUpdater(myVehicle->pNode));
//    viewer->setSceneData(myWorld->scene);


    //创建主场景的副视角
    CreateSubwid();



    timer=new QTimer(this);
    realTimer=new QElapsedTimer;
    TimergetSpan=new QElapsedTimer;
    connect(timer, &QTimer::timeout, this, QOverload<>::of(&MainWindow::update));
    timer2 = new QTimer(this);


    connect(timer2, &QTimer::timeout, this, QOverload<>::of(&MainWindow::realTimeUpdate));

    timer->start(16);
    timer2->start(5);

    timer3 = new QTimer(this);
    connect(timer3, &QTimer::timeout, this, QOverload<>::of(&MainWindow::CalTraj));
    timer2->start(0);


    realTimer->start();

    //下面在进行一些绑定
    //最开始是模式的转换
    //只要按下按钮，就开始读取键盘数据
    connect(ui->action_manCtl, SIGNAL(triggered()), this, SLOT(SlotStartThread()));
    //这个连接的就是按钮和通过算法自动规划路径的开关
    connect(ui->action_selfCtl, SIGNAL(triggered()), this, SLOT(changePurepursuit()));

    //发送成功规划路径的信号
//    connect(SignalFactory::SignalInstance().get(), SIGNAL(PlanSuccessInfo()), this, SLOT(InfoBox()), Qt::QueuedConnection);
    connect(SignalFactory::SignalInstance().get(), &SignalFactory::PlanSuccessInfo, this, &MainWindow::InfoBox, Qt::QueuedConnection);
    connect(SignalFactory::SignalInstance().get(), &SignalFactory::PlanFailInfo, this, &MainWindow::InfoBoxFail, Qt::QueuedConnection);





}

MainWindow::~MainWindow()
{
    delete ui;

}

void MainWindow::initStyle()
{
    QFile file(":/qss/lightblue.css");
    if (file.open(QFile::ReadOnly)) {
        QString qss = QLatin1String(file.readAll());
        QString paletteColor = qss.mid(20, 7);
        qApp->setPalette(QPalette(QColor(paletteColor)));
        qApp->setStyleSheet(qss);
        file.close();
    }
}

void MainWindow::update()
{

    GameWorld::getHandle()->update();

}

void MainWindow::realTimeUpdate()
{
    dt = realTimer->elapsed() - lastTime;

    myWorld->stepSimulation(dt,1);
//    this->myVehicle->subPower();
//    myVehicle->setPower(23);
    myVehicle->slowErasePower();
    this->ui->lineEdit_px->setText("("+QString::number(myVehicle->vehicle->getChassisWorldTransform().getOrigin().x(),'f',1) +","+QString::number(myVehicle->vehicle->getChassisWorldTransform().getOrigin().y(),'f',1) +")");
//    this->ui->lineEdit_py->setText(""+QString::number(myVehicle->vehicle->getChassisWorldTransform().getOrigin().y(),'f',1) +"(positionY)");
    this->ui->lineEdit_velocity->setText(QString::number(myVehicle->vehicle->getCurrentSpeedKmHour(),'f',1)+"km/h");
    std::vector<double> a=this->myVehicle->getEulerAngle();
    this->ui->lineEdit_headingAngle->setText(""+QString::number(-a[0],'f',1)+"°");
    //这里我想的是把它的动力和速度可视化
    this->ui->widget_dashboard->setValue(myVehicle->vehicle->getCurrentSpeedKmHour());
    this->ui->widget_waterprogressbar->setValue(static_cast<int>(this->myVehicle->powerDegree));






    lastTime = realTimer->elapsed();
}

void MainWindow::CreateSubwid()
{
    osg_widget1 = new QtOSGWidget();

    osg_widget1 = ui->openGLWidget_2;

    viewer1 = osg_widget1->getViewer();

    GameWorld::getHandle()->setViewer(viewer1);

//    btosgVec3 up(0., 0., 1.);
//    osg::Matrix matrix;
//    viewer->getCamera()->setViewMatrix(matrix);

    viewer1->getCamera()->addChild( myWorld->scene );

    viewer1->setCameraManipulator( GameWorld::getHandle()->manipulator );

    viewer1->addEventHandler(new FollowUpdater(myVehicle->pNode));
}

void MainWindow::InfoBox()
{
//    qDebug()<<"chang shi yi xia 看一看传没传过来,\n ";
//    TimergetSpan->start();
    qint64 elapsedTime = TimergetSpan->elapsed();
    QMessageBox::information(this, "Plan Result", "规划成功!用时" + QString::number(elapsedTime) + "ms");
//    QMessageBox::information(this,"Plan Result","规划成功!");
    ab->pointNum = SignalFactory::SignalInstance()->m_PointNum;
//    qDebug()<<"12324124124124"<< ab->pointNum <<endl;
    ab->caj = ab->CreateLine(SignalFactory::SignalInstance()->m_PointArray,SignalFactory::SignalInstance()->m_PointNum);
    myWorld->scene->addChild(ab->caj);
}
void MainWindow::InfoBoxFail()
{
    //    qDebug()<<"chang shi yi xia 看一看传没传过来,\n ";
    //    TimergetSpan->start();
    qint64 elapsedTime = TimergetSpan->elapsed();
    QMessageBox::information(this, "Plan error", "规划失败，存在不可达点");

}


void MainWindow::SlotStartThread()
{
    timer3->stop();
    myVehicle->setMass(800.);
    if(threadOsg.joinable())
    {
        threadOsg.detach();
    }
}

void MainWindow::changePurepursuit()
{
    //这里应该让文本输入框可用
    ui->lineEdit_sx->setEnabled(true);
    ui->lineEdit_sy->setEnabled(true);
    ui->lineEdit_ex->setEnabled(true);
    ui->lineEdit_ey->setEnabled(true);

    ui->lineEdit_sx->setPlaceholderText("起始点(x)");
    ui->lineEdit_sy->setPlaceholderText("起始点(y)");
    ui->lineEdit_ex->setPlaceholderText("终止点(x)");
    ui->lineEdit_ey->setPlaceholderText("终止点(y)");



    myVehicle->test(btosgVec3(0,0,1));
    myVehicle->vehicle->applyEngineForce(5, 2);
    myVehicle->vehicle->applyEngineForce(5, 3);
    myVehicle->vehicle->setSteeringValue(btScalar(0), 0);
    myVehicle->vehicle->setSteeringValue(btScalar(0), 1);
    ab = new purepursuit(myVehicle);
}

void MainWindow::CalTraj()
{
    if(ab->end == 0)
    {

        myVehicle->vehicle->setBrake(1000000000,2);
        myVehicle->vehicle->setBrake(1000000000,3);
        ab->PidControl(0);

    }else
    {


         ab->start_pursuit(SignalFactory::SignalInstance()->locateX,SignalFactory::SignalInstance()->locateY,myVehicle);
    }

}


void MainWindow::on_pushButton_planTraj_clicked()
{
    double sx, sy, ex, ey;

    bool ok;

    sx = ui->lineEdit_sx->text().toDouble(&ok);
    if (!ok) {
        QMessageBox::warning(this, "输入错误", "起点 x 坐标无效，请重新输入！");
        return;
    }

    sy = ui->lineEdit_sy->text().toDouble(&ok);
    if (!ok) {
        QMessageBox::warning(this, "输入错误", "起点 y 坐标无效，请重新输入！");
        return;
    }

    ex = ui->lineEdit_ex->text().toDouble(&ok);
    if (!ok) {
        QMessageBox::warning(this, "输入错误", "终点 x 坐标无效，请重新输入！");
        return;
    }

    ey = ui->lineEdit_ey->text().toDouble(&ok);
    if (!ok) {
        QMessageBox::warning(this, "输入错误", "终点 y 坐标无效，请重新输入！");
        return;
    }
//    if(m_thread->almaze[sx+ 1735][sy+ 1734]==1||m_thread->almaze[ex+ 1735][ey+ 1734]==1){
//        QMessageBox::warning(this, "输入错误", "输入坐标存在不可达点！");
//        return;
//    }
    TimergetSpan->start();
    emit SignalFactory::SignalInstance()->StartPlan(sx, sy, ex, ey);


}

void MainWindow::on_pushButton_eliminateTraj_clicked()
{
    myWorld->scene->removeChild(ab->caj);

}

void MainWindow::on_pushButton_trajTrack_clicked()
{

    myVehicle->test(btosgVec3(SignalFactory::SignalInstance()->locateX[0],SignalFactory::SignalInstance()->locateY[0],1.5));
    ab->target_ind = ab->calc_target_index(SignalFactory::SignalInstance()->locateX,SignalFactory::SignalInstance()->locateY);
    ab->start_pursuit(SignalFactory::SignalInstance()->locateX,SignalFactory::SignalInstance()->locateY,myVehicle);
    timer3->start(100);
}

void MainWindow::on_pushButton_trajStop_clicked()
{
    timer3->stop();
    ab->PidControl(ab->target_speed);
    myVehicle->vehicle->setSteeringValue(btScalar(0), 0);
    myVehicle->vehicle->setSteeringValue(btScalar(0), 1);
}

void MainWindow::on_pushButton_mainView_clicked()
{
    viewer->setCameraManipulator( GameWorld::getHandle()->manipulator1 );
    viewer1->setCameraManipulator( GameWorld::getHandle()->manipulator );
}

void MainWindow::on_pushButton_subView_clicked()
{
    viewer->setCameraManipulator(GameWorld::getHandle()->manipulator);
    viewer1->setCameraManipulator( GameWorld::getHandle()->manipulator1 );
}

void MainWindow::on_pushButton_applyEngine_clicked()
{
    bool ok = false;
    double targetSpeed = ui->lineEdit_velo->text().toDouble(&ok);
    if (!ok || targetSpeed <= 0) {
        QMessageBox::warning(this, "输入错误", "输入的目标速度无效，请重新输入！");
        return;
    }
    ab->target_speed = targetSpeed;



}

void MainWindow::on_pushButton_applyForce_clicked()
{

    bool ok = false;
    double newton = ui->lineEdit_new->text().toDouble(&ok);
    if (!ok || newton <= 0) {
        QMessageBox::warning(this, "输入错误", "输入的驱动力无效，请重新输入！");
        return;
    }
    ab->newton = newton;

}

void MainWindow::on_pushButton_applyWeight_clicked()
{
    bool ok=false;
    double mass = ui->lineEdit_mass->text().toDouble(&ok);
    if (!ok||mass<=0) {

        QMessageBox::warning(this, "输入错误", " 输入的重量无效，请重新输入！");
        return;
    }
    myVehicle->setMass(210 + mass * 30);
    myVehicle->setMass(210 + ui->lineEdit_mass->text().toDouble()*30);
}

void MainWindow::on_pushButton_posOrigin_clicked()
{
    myVehicle->test(btosgVec3(0,0,0));
}

void MainWindow::on_pushButton_posCrater_clicked()
{
    myVehicle->test(btosgVec3(283,70,-12));
}

void MainWindow::on_action_manCtl_triggered()
{
    //这里应该让文本输入框可用
    ui->lineEdit_sx->setEnabled(false);
    ui->lineEdit_sy->setEnabled(false);
    ui->lineEdit_ex->setEnabled(false);
    ui->lineEdit_ey->setEnabled(false);

    ui->lineEdit_sx->setPlaceholderText("disable");
    ui->lineEdit_sy->setPlaceholderText("disable");
    ui->lineEdit_ex->setPlaceholderText("disable");
    ui->lineEdit_ey->setPlaceholderText("disable");
}
